
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */

#include "implementation.h"
#include "../captain/capser.h"

#define GET_STATUS		1
#define GET_POSITION	2
#define GET_TEMPERATURE	3
#define GET_CAMERA		4
#define MOVE_X			5
#define MOVE_Y			6
#define MOVE_Z			7
#define ACK				10
#define RET_STATUS		11
#define RET_POSITION	12
#define RET_TEMPERATURE	13
#define RET_CAMERA		14
#define ACK_MOVE_X		15
#define ACK_MOVE_Y		16
#define ACK_MOVE_Z		17

int fd;
char VERSION = 1 ;
int SEGMENT_NUMBER = 0 ;
int TIMEOUT = 500 ; // en millisecondes


int returnStatus() {
	// TO DO
	return 0 ;
}

int returnPosition() {
	// TO DO
	return 0 ;
}

char * returnTemperature() {
	//TO DO
	return 0;
}

int returnCamera() {
	// TO DO
	return 0 ;
}

int moveX(int newCoordinate) {
	// TO DO
	return 0 ;
}

int moveY(int newCoordinate) {
	// TO DO
	return 0 ;
}

int moveZ(int newCoordinate) {
	// TO DO
	return 0
}

int aknowledge(fd, cmd) {
	char data[32] ;
	char * msg = null ;
	
	msg = Encode(cmd, data);
	writeport(fd, msg);	
	
	return -1 ;
}

char * encode(char cmd, char * data) {
	
	unsigned short totalLength = strlen(data) + 8 ;
	char * msg = malloc(sizeof(char)*totalLength) ;
	int checkSumLSB, checkSumMSB ;
	SEGMENT_NUMBER++ ;
	
	// Champ Version 
	msg[0] = VERSION ;
	printf("VERSION  : %c", msg[0]);
	
	// Champ Total Length 
	msg[1] = (totalLength && 0xFF00) >> 8 ; // MSB
	msg[2] = totalLength && 0xFF ; // LSB
	printf("TotalLength  : %c|%c", msg[1], msg[2]);
	
	// Champ CMD
	msg[3] = cmd ;
	printf("CMD  : %c", msg[3]);
	
	// Champ Segment #
	msg[4] = (SEGMENT_NUMBER && 0xFF00) >> 8 ; // MSB
	msg[5] = SEGMENT_NUMBER && 0xFF ; // LSB
	printf("Segment #  : %c|%c", msg[4], msg[5]);
	
	// Champ Checksum
	checkSumLSB = (msg[0]*msg[1]*msg[2]*msg[3]*msg[4]*msg[5])%0xFF ;
	checkSumMSB =  (msg[0]+msg[1]+msg[2]+msg[3]+msg[4]+msg[5])%0xFF ;
	msg[6] = (checkSumMSB && 0xFF00) >> 8 ; // MSB
	msg[7] = checkSumLSB && 0xFF ; // LSB
	printf("Segment #  : %c|%c", msg[6], msg[7]);
	
	// Data
	strcat(msg,data);
	
	printf("MSG  : %s", msg);

	return msg ;
}

char * decode(char * message){
	char version = message[0];
	int length = ((int)message[1])*10+(int)message[2];
	char cmd = (int)message[3];
	int num_segment = ((int)message[4])*10+(int)message[5];
	int chacksum = ((int)message[6])*10+(int)message[7];
	char * data = malloc(sizeof(char *)*(length-8)) ;

	int fd = open("/dev/ttyS1", O_RDWR | O_NOCTTY | O_NDELAY);
	
	char * retour;
	int retour2;
	
	int i, j = 0;

	for (int i=9; i < length; i++)
	{
		data[j] = message[i];
		j++;
	}

	switch (cmd)
	{
		case GET_STATUS:
			retour = returnStatus();
			msg = Encode(RETURN_STATUS, retour);
			writeport(fd, msg);
			break;
		case GET_POSITION:
			retour = returnPosition();
			msg = Encode(RETURN_POSITION, retour);
			writeport(fd, msg);
			break;
		case GET_TEMPERATURE:
			retour = returnTemperature();
			msg = Encode(RETURN_TEMPERATURE, retour);
			writeport(fd, msg);
			break;
		case GET_CAMERA:
			retour = returnCamera();
			msg = Encode(RETURN_CAMERA, retour);
			writeport(fd, msg);
			break;
		case MOVE_X: 
			retour2 = moveX((int)data);
			if (retour2 == 0)
				aknowledge(fd, ACK_MOVE_X);
			break;
		case MOVE_Y:
			retour2 = moveY((int)data);
			if (retour2 == 0)
				aknowledge(fd, ACK_MOVE_Y);
			break;
		case MOVE_Z:
			retour2 = moveZ((int)data);
			if (retour2 == 0)
				aknowledge(fd, ACK_MOVE_Z);
			break;
		case 8:
			//defineOcc
			break;
		case 10:
			acknowledge(fd, ACK);
			break;
		default:
			break;
	}
	close (fd);
}